#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#define MIN_RADIUS 1.0e-4f

static float _two_points_distance(const cv::Point2f& pt1, const cv::Point2f& pt2) {
    float dx = pt1.x - pt2.x;
    float dy = pt1.y - pt2.y;
    float d = (float)std::sqrt(dx*dx+dy*dy);
    return d;
}

static void _find_circle_3pts(const cv::Point2f* ptsf, cv::Point2f& center, float& radius) {
    cv::Point2f v1 = ptsf[1] - ptsf[0];
    cv::Point2f v2 = ptsf[2] - ptsf[0];
    
    cv::Point2f mid1 = (ptsf[0] + ptsf[1]) / 2.f;
    float c1 = mid1.x*v1.x + mid1.y*v1.y;
    cv::Point2f mid2 = (ptsf[0] + ptsf[2]) / 2.f;
    float c2 = mid2.x*v2.x + mid2.y*v2.y;
    float det = v1.x*v2.y - v1.y*v2.x;
    if (std::fabs(det) <= MIN_RADIUS) {
        float d1 = _two_points_distance(ptsf[0], ptsf[1]);
        float d2 = _two_points_distance(ptsf[0], ptsf[2]);
        float d3 = _two_points_distance(ptsf[2], ptsf[2]);
        if (d1 >= d2 && d1 >= d3) {
            center = (ptsf[0] + ptsf[1]) / 2.f;
        } else if (d2 >= d1 && d2 >= d3) {
            center = (ptsf[0] + ptsf[2]) / 2.f;
        } else {
            CV_Assert(d3 >= d1 && d3 >= d2);
            center = (ptsf[1] + ptsf[2]) / 2.f;
        }
        return;
    }
    
    center.x = (c1*v2.y-c2*v1.y) / det;
    center.y = (v1.x*c2-v2.x*c1) / det;
    float dx = center.x - ptsf[0].x;
    float dy = center.y - ptsf[0].y;
    radius = (float)(std::sqrt(dx*dx+dy*dy)) + MIN_RADIUS;
}

static void _find_third_point(const std::vector<cv::Point>& pts, int i, int j, cv::Point2f& center, float& radius) {
    center.x = (float)(pts[j].x + pts[i].x) / 2.f;
    center.y = (float)(pts[j].y + pts[i].y) / 2.f;
    float dx = (float)(pts[j].x - pts[i].x);
    float dy = (float)(pts[j].y - pts[i].y);
    radius = (float)std::sqrt(dx*dx+dy*dy)/2.f + MIN_RADIUS;
    for (int k = 0; k < j; k++) {
        dx = center.x - (float)pts[k].x;
        dy = center.y - (float)pts[k].y;
        if (std::sqrt(dx*dx+dy*dy) < radius) {
            continue;
        } else {
            cv::Point2f ptsf[3];
            ptsf[0] = cv::Point2f(pts[i].x, pts[i].y);
            ptsf[1] = cv::Point2f(pts[j].x, pts[j].y);
            ptsf[2] = cv::Point2f(pts[k].x, pts[k].y);
            cv::Point2f newCenter;
            float newRadius = 0;
            _find_circle_3pts(ptsf, newCenter, newRadius);
            if (newRadius > 0) {
                radius = newRadius;
                center = newCenter;
            }
        }
    }
}

static void _find_second_point(const std::vector<cv::Point>& pts, int i, cv::Point2f& center, float& radius) {
    center.x = (float)(pts[0].x + pts[i].x)/2.f;
    center.y = (float)(pts[0].y + pts[i].y)/2.f;
    float dx = (float)(pts[0].x - pts[i].x);
    float dy = (float)(pts[0].y - pts[i].y);
    radius = (float)std::sqrt(dx*dx+dy*dy)/2.f + MIN_RADIUS;
    for (int j = 1; j < i; j++) {
        dx = center.x - (float)pts[j].x;
        dy = center.y - (float)pts[j].y;
        if (std::sqrt(dx*dx+dy*dy) < radius) {
            continue;
        } else {
            cv::Point2f newCenter;
            float newRadius = 0;
            _find_third_point(pts, i, j, newCenter, newRadius);
            if (newRadius > 0) {
                radius = newRadius;
                center = newCenter;
            }
        }
    }
}

static void _min_enclosing_circle(const std::vector<cv::Point>& pts, cv::Point2f& center, float& radius) {
    center.x = center.y = 0.f;
    radius = 0.f;
    int count = pts.size();
    if (count == 0) {
        return;
    }
    
    if (count == 1) {
        center = cv::Point2f((float)pts[0].x, (float)pts[0].y);
        radius = MIN_RADIUS;
    } else if (count == 2) {
        cv::Point2f p1 = cv::Point2f((float)pts[0].x, (float)pts[0].y);
        cv::Point2f p2 = cv::Point2f((float)pts[1].x, (float)pts[1].y);
        center.x = (p1.x+p2.x) / 2.f;
        center.y = (p1.y+p2.y) / 2.f;
        double dx = p1.x-p2.x;
        double dy = p1.y-p2.y;
        radius = (float)(std::sqrt(dx*dx+dy*dy)/2.) + MIN_RADIUS;
    } else {
        center.x = (float)(pts[0].x + pts[1].x) / 2.f;
        center.y = (float)(pts[0].y + pts[1].y) / 2.f;
        float dx = (float)(pts[0].x - pts[1].x);
        float dy = (float)(pts[0].y - pts[1].y);
        radius = (float)std::sqrt(dx*dx+dy*dy) / 2.f + MIN_RADIUS;
        for (int i = 2; i < count; i++) {
            dx = (float)pts[i].x - center.x;
            dy = (float)pts[i].y - center.y;
            float d = (float)std::sqrt(dx*dx+dy*dy);
            if (d < radius) {
                continue;
            } else {
                cv::Point2f newCenter;
                float newRadius = 0;
                _find_second_point(pts, i, newCenter, newRadius);
                if (newRadius > 0) {
                    radius = newRadius;
                    center = newCenter;
                }
            }
        }
    }
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    cv::Mat binary;
    //cv::Canny(src, binary, 250, 1000);
    cv::Canny(src, binary, 250, 600);
    
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binary, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    std::vector<cv::Point2f> centers, centers2;
    std::vector<float> radius, radius2;
    for (int i = 0; i < contours.size(); i++) {
        cv::Point2f c;
        float r;
        cv::minEnclosingCircle(contours[i], c, r);
        centers.push_back(c);
        radius.push_back(r);
        for (int j = 0; j < contours[i].size(); j++) {
            cv::circle(src, contours[i][j], 1, cv::Scalar(255, 0, 0), cv::FILLED);
        }
        _min_enclosing_circle(contours[i], c, r);
        centers2.push_back(c);
        radius2.push_back(r);
    }
    for (int i = 0; i < centers.size(); i++) {
        std::cout << "Circle " << i << ": center-> " << centers[i] << ", radius-> " << radius[i] << std::endl;
        cv::circle(src, centers2[i], cvRound(radius2[i]), cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
    }
    
    bool allSame = true;
    for (int i = 0; i < centers.size(); i++) {
        if (std::fabs(centers[i].x - centers2[i].x) > FLT_EPSILON ||
            std::fabs(centers[i].y - centers2[i].y) > FLT_EPSILON || 
            std::fabs(radius[i] - radius2[i]) > FLT_EPSILON) {
            allSame = false;
            break;
        }
    }
    std::cout << "All results are " << (allSame ? "the same!" : "not the same!") << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("binary", binary);
    cv::waitKey(0);
    return 0;
}
